#ifndef XM430_DRIVER_HPP
#define XM430_DRIVER_HPP

#define PROTOCOL_VERSION 2.0
#define BAUDRATE 1000000

#define ADDR_OPERATING_MODE 11
#define ADDR_TORQUE_ENABLE 64
#define ADDR_GOAL_VELOCITY 104
#define ADDR_GOAL_POSITION 116
#define ADDR_PRESENT_VELOCITY 128
#define ADDR_PRESENT_POSITION 132

#define RAD_TO_DXL_POSITION 651.088663664 // 1 / 0.0174533 / 0.088
#define RAD_S_TO_RPM 9.549 // 60 / (2*PI)
#define RPM_TO_DXL_VELOCITY 4.366812227 // 1 / 0.229

#define OPERATING_MODE_VELOCITY 1
#define OPERATING_MODE_POSITION 3

#include <dynamixel_sdk/dynamixel_sdk.h>
#include <iostream>

class XM430Driver {
public:
    XM430Driver(std::string device_name):
        portHandler_(dynamixel::PortHandler::getPortHandler(device_name.c_str())),
        packetHandler_(dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION)),
        groupBulkRead_(portHandler_, packetHandler_),
        groupSyncWrite_(portHandler_, packetHandler_, ADDR_GOAL_POSITION, 4)
    {

    }

    int init(){
        std::cout << "Initializing connection with robot." << std::endl;

        // Open port
        if (portHandler_->openPort()) {
            std::cout << "Succeeded to open the port!" << std::endl;
        }
        else{
            std::cout << "Failed to open the port!" << std::endl;
            return -1;
        }

        // Set port baudrate
        if (portHandler_->setBaudRate(BAUDRATE)) {
            std::cout << "Succeeded to change the baudrate!" << std::endl;
        }
        else {
            std::cout << "Failed to change the baudrate!" << std::endl;
            return -1;
        }
        portHandler_->setPacketTimeout(0.1); 
        return 0;
    }

    void activateWithPositionMode(int dxl_id)
    {
        std::cout << "Activate motor" << std::endl;

        // Set Position Control Mode
        packetHandler_->write1ByteTxRx(portHandler_, dxl_id, ADDR_OPERATING_MODE, OPERATING_MODE_POSITION);

        // Enable Torque
        packetHandler_->write1ByteTxRx(portHandler_, dxl_id, ADDR_TORQUE_ENABLE, 1);
    }

    void activateWithVelocityMode(int dxl_id)
    {
        std::cout << "Activate motor" << std::endl;

        // Set Velocity Control Mode
        packetHandler_->write1ByteTxRx(portHandler_, dxl_id, ADDR_OPERATING_MODE, OPERATING_MODE_VELOCITY);

        // Enable Torque
        packetHandler_->write1ByteTxRx(portHandler_, dxl_id, ADDR_TORQUE_ENABLE, 1);
    }

    void deactivate(int dxl_id)
    {
        std::cout << "Deactivate motor" << std::endl;

        // Disable Torque
        packetHandler_->write1ByteTxRx(portHandler_, dxl_id, ADDR_TORQUE_ENABLE, 0);
    }

    void setTargetPositionRadian(int dxl_id, double command)
    {
        int dxl_cmd = command * RAD_TO_DXL_POSITION + 2048;
        packetHandler_->write4ByteTxRx(portHandler_, dxl_id, ADDR_GOAL_POSITION, dxl_cmd);
    }

    void setTargetVelocityRadianPerSec(int dxl_id, double command)
    {
        int dxl_cmd = command * RAD_S_TO_RPM * RPM_TO_DXL_VELOCITY;
        packetHandler_->write4ByteTxRx(portHandler_, dxl_id, ADDR_GOAL_VELOCITY, dxl_cmd);
    }

    double getPositionRadian(int dxl_id)
    {
        int32_t dxl_present_position = 0;
        packetHandler_->read4ByteTxRx(portHandler_, dxl_id, ADDR_PRESENT_POSITION, (uint32_t*)&dxl_present_position);
        return (double)(dxl_present_position - 2048) / RAD_TO_DXL_POSITION;
    }

    double getVelocityRadianPerSec(int dxl_id)
    {
        int32_t dxl_present_velocity = 0;
        packetHandler_->read4ByteTxRx(portHandler_, dxl_id, ADDR_PRESENT_VELOCITY, (uint32_t*)&dxl_present_velocity);
        double velocity = (double)dxl_present_velocity / RPM_TO_DXL_VELOCITY / RAD_S_TO_RPM ;
        return velocity;
    }

// +++ 添加这个新函数 +++
    // 在 on_configure() 中调用它
    bool initBulkSync(int dxl_id_1, int dxl_id_2, int dxl_id_3, int dxl_id_4, int dxl_id_5)
    {
        // 初始化批量读 (用于 PRESENT_POSITION)
        groupBulkRead_ = dynamixel::GroupBulkRead(portHandler_, packetHandler_);
        if (!groupBulkRead_.addParam(dxl_id_1, ADDR_PRESENT_POSITION, 4)) return false;
        if (!groupBulkRead_.addParam(dxl_id_2, ADDR_PRESENT_POSITION, 4)) return false;
        if (!groupBulkRead_.addParam(dxl_id_3, ADDR_PRESENT_POSITION, 4)) return false;
        if (!groupBulkRead_.addParam(dxl_id_4, ADDR_PRESENT_POSITION, 4)) return false;
        if (!groupBulkRead_.addParam(dxl_id_5, ADDR_PRESENT_POSITION, 4)) return false;

        // 初始化同步写 (用于 GOAL_POSITION, 长度 4 字节)
        groupSyncWrite_ = dynamixel::GroupSyncWrite(portHandler_, packetHandler_, ADDR_GOAL_POSITION, 4);

        return true;
    }

    // +++ 添加这个新函数 +++
    // 批量读取两个舵机的位置
    bool readPresentPositions(int dxl_id_1, double &pos_1, int dxl_id_2, double &pos_2, 
        int dxl_id_3, double &pos_3, int dxl_id_4, double &pos_4, int dxl_id_5, double &pos_5)
    {
        int dxl_comm_result = groupBulkRead_.txRxPacket();
        if (dxl_comm_result != COMM_SUCCESS) {
            packetHandler_->getTxRxResult(dxl_comm_result);
            return false;
        }

        // 检查数据是否返回
        if (!groupBulkRead_.isAvailable(dxl_id_1, ADDR_PRESENT_POSITION, 4)) return false;
        if (!groupBulkRead_.isAvailable(dxl_id_2, ADDR_PRESENT_POSITION, 4)) return false;
        if (!groupBulkRead_.isAvailable(dxl_id_3, ADDR_PRESENT_POSITION, 4)) return false;
        if (!groupBulkRead_.isAvailable(dxl_id_4, ADDR_PRESENT_POSITION, 4)) return false;
        if (!groupBulkRead_.isAvailable(dxl_id_5, ADDR_PRESENT_POSITION, 4)) return false;

        // 获取数据
        int32_t dxl_present_pos_1 = groupBulkRead_.getData(dxl_id_1, ADDR_PRESENT_POSITION, 4);
        int32_t dxl_present_pos_2 = groupBulkRead_.getData(dxl_id_2, ADDR_PRESENT_POSITION, 4);
        int32_t dxl_present_pos_3 = groupBulkRead_.getData(dxl_id_3, ADDR_PRESENT_POSITION, 4);
        int32_t dxl_present_pos_4 = groupBulkRead_.getData(dxl_id_4, ADDR_PRESENT_POSITION, 4);
        int32_t dxl_present_pos_5 = groupBulkRead_.getData(dxl_id_5, ADDR_PRESENT_POSITION, 4);

        // 转换为弧度
        pos_1 = (double)(dxl_present_pos_1 - 2048) / RAD_TO_DXL_POSITION;
        pos_2 = (double)(dxl_present_pos_2 - 2048) / RAD_TO_DXL_POSITION;
        pos_3 = (double)(dxl_present_pos_3 - 2048) / RAD_TO_DXL_POSITION;
        pos_4 = (double)(dxl_present_pos_4 - 2048) / RAD_TO_DXL_POSITION;
        pos_5 = (double)(dxl_present_pos_5 - 2048) / RAD_TO_DXL_POSITION;

        return true;
    }

    // +++ 添加这个新函数 +++
    // 同步写入两个舵机的位置
    void writeTargetPositions(int dxl_id_1, double cmd_1, int dxl_id_2, double cmd_2, 
        int dxl_id_3, double cmd_3, int dxl_id_4, double cmd_4, int dxl_id_5, double cmd_5)
    {
        // 转换单位
        int32_t dxl_cmd_1 = cmd_1 * RAD_TO_DXL_POSITION + 2048;
        int32_t dxl_cmd_2 = cmd_2 * RAD_TO_DXL_POSITION + 2048;
        int32_t dxl_cmd_3 = cmd_3 * RAD_TO_DXL_POSITION + 2048;
        int32_t dxl_cmd_4 = cmd_4 * RAD_TO_DXL_POSITION + 2048;
        int32_t dxl_cmd_5 = cmd_5 * RAD_TO_DXL_POSITION + 2048;

        // 准备舵机1的数据 (4字节)
        uint8_t param_goal_position_1[4];
        param_goal_position_1[0] = DXL_LOBYTE(DXL_LOWORD(dxl_cmd_1));
        param_goal_position_1[1] = DXL_HIBYTE(DXL_LOWORD(dxl_cmd_1));
        param_goal_position_1[2] = DXL_LOBYTE(DXL_HIWORD(dxl_cmd_1));
        param_goal_position_1[3] = DXL_HIBYTE(DXL_HIWORD(dxl_cmd_1));
        groupSyncWrite_.addParam(dxl_id_1, param_goal_position_1);

        // 准备舵机2的数据 (4字节)
        uint8_t param_goal_position_2[4];
        param_goal_position_2[0] = DXL_LOBYTE(DXL_LOWORD(dxl_cmd_2));
        param_goal_position_2[1] = DXL_HIBYTE(DXL_LOWORD(dxl_cmd_2));
        param_goal_position_2[2] = DXL_LOBYTE(DXL_HIWORD(dxl_cmd_2));
        param_goal_position_2[3] = DXL_HIBYTE(DXL_HIWORD(dxl_cmd_2));
        groupSyncWrite_.addParam(dxl_id_2, param_goal_position_2);

        // 准备舵机3的数据 (4字节)
        uint8_t param_goal_position_3[4];
        param_goal_position_3[0] = DXL_LOBYTE(DXL_LOWORD(dxl_cmd_3));
        param_goal_position_3[1] = DXL_HIBYTE(DXL_LOWORD(dxl_cmd_3));
        param_goal_position_3[2] = DXL_LOBYTE(DXL_HIWORD(dxl_cmd_3));
        param_goal_position_3[3] = DXL_HIBYTE(DXL_HIWORD(dxl_cmd_3));
        groupSyncWrite_.addParam(dxl_id_3, param_goal_position_3);

        // 准备舵机4的数据 (4字节)
        uint8_t param_goal_position_4[4];
        param_goal_position_4[0] = DXL_LOBYTE(DXL_LOWORD(dxl_cmd_4));
        param_goal_position_4[1] = DXL_HIBYTE(DXL_LOWORD(dxl_cmd_4));
        param_goal_position_4[2] = DXL_LOBYTE(DXL_HIWORD(dxl_cmd_4));
        param_goal_position_4[3] = DXL_HIBYTE(DXL_HIWORD(dxl_cmd_4));
        groupSyncWrite_.addParam(dxl_id_4, param_goal_position_4);

        // 准备舵机5的数据 (4字节)
        uint8_t param_goal_position_5[4];
        param_goal_position_5[0] = DXL_LOBYTE(DXL_LOWORD(dxl_cmd_5));
        param_goal_position_5[1] = DXL_HIBYTE(DXL_LOWORD(dxl_cmd_5));
        param_goal_position_5[2] = DXL_LOBYTE(DXL_HIWORD(dxl_cmd_5));
        param_goal_position_5[3] = DXL_HIBYTE(DXL_HIWORD(dxl_cmd_5));
        groupSyncWrite_.addParam(dxl_id_5, param_goal_position_5);

        // 发送一个同步写数据包
        groupSyncWrite_.txPacket();

        // 清除参数列表，为下一次循环做准备
        groupSyncWrite_.clearParam();
    }

private:
    dynamixel::PortHandler *portHandler_;
    dynamixel::PacketHandler *packetHandler_;
// +++ 添加这两个成员变量 +++
    dynamixel::GroupBulkRead groupBulkRead_;
    dynamixel::GroupSyncWrite groupSyncWrite_;
};

#endif
